import lejos.navigation.TachoNavigator;
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.subsumption.Arbitrator;
import lejos.subsumption.Behavior;
import lejos.util.Timer;


public class TestBehavior {
	private TachoNavigator tn;
	private CalibratedLS cls;
	private Arbitrator arby;
	
	public TestBehavior(TachoNavigator tn, CalibratedLS cls) {
		this.tn = tn;
		this.cls = cls;

		Behavior[] bArray = { b3, b2, b1 };
		arby = new Arbitrator(bArray);

		Button.waitForPress();
		
		arby.start();
	}
	
	public static void main(String[] args) {
		SensorPort LightSensorPort = SensorPort.S1;
		Motor LeftMotor = Motor.B;
		Motor RightMotor = Motor.A;

		CalibratedLS cls = new CalibratedLS(LightSensorPort, true);
		//cls.calibrate();

		TachoNavigator tn = new TachoNavigator(8.16f, 14.15f, LeftMotor, RightMotor);
		tn.setSpeed(200);

		new TestBehavior(tn, cls);
	}
	
	Behavior b1 = new Behavior() {
		public void action() {
			Sound.beep();
			tn.forward();
		}

		public void suppress() {
			tn.stop();
		}

		public boolean takeControl() {
			return false;
		}		
	};
	
	Behavior b2 = new Behavior() {
		public void action() {
			Sound.twoBeeps();
			tn.backward();
		}

		public void suppress() {
			tn.stop();
		}

		public boolean takeControl() {
			return false;
		}		
	};
	
	Behavior b3 = new Behavior() {
		public void action() {
			Sound.beep();
			tn.rotate(45, true);
		}

		public void suppress() {
			tn.stop();
		}

		public boolean takeControl() {
			return true;
		}		
	};	
}
